/** \file   main.c
 *
 *  \brief  This file contains main function.
 *
 *   This file performs functions on the following:
 *
 *      1. Procedure to set up system
 *      2. Procedure to set up peripheral drivers
 *      3. Procedure for central control
 *      4. user defined driver initialization
 *
 *   Warning: This file should not use optimization levels other than 0.
 *
 *  \author     Jiawei Xie
 *
 *  \version    0.1   Created newly for NAVCON
 */


/* ========================================================================== */
/*                          Main FILE                    		              */
/* ========================================================================== */



#include "funcdef.h"
#include "navconbioscfg.h"
#include "..\libNav\cppexample.h"

/* Internal data structure in .data section*/
/* System Control Block */
SYS_Ctrl_b SCB;
NavDataObj vNavData;
int tsk_ctrl = 0;



void main()
{
	int status = 0;
	I2c_ChanParams     I2C_chanParams;
	Uart_ChanParams    UART_chanParams;
    /* pinmux and evm related configurations */
	//SYS_Config();
	/* configure PSP drivers*/
    configureUart();
    configureI2c();

    //cppexample();

    /* system */
    SCB.trace = &sys_trace;
	SCB.TSK_ENA[TSK_BMA180] 	= 	FALSE;
	SCB.TSK_ENA[TSK_ITG3205] 	= 	FALSE;
	SCB.TSK_ENA[TSK_MS5611] 	= 	FALSE;
	SCB.TSK_ENA[TSK_HMC5883L] 	= 	FALSE;
	SCB.TSK_ENA[TSK_ctrl_term] 	= 	FALSE;
	SCB.TSK_ENA[TSK_datalog] 	= 	FALSE;
	SCB.TSK_ENA[TSK_GPS]		=   FALSE;
	SCB.TSK_ENA[MATRIX_TEST]	=   FALSE;
	SCB.INITIALISED 			= 	FALSE;


    status = register_tsks(MATRIX_TEST, &sys_trace, NULL, NULL, NULL, NULL);
    if(status){
    	//DEBUG
#ifdef _DEBUG
    	LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_ctrl_term);
#endif
    }

    /* Set up task control blocks */
	UART_chanParams.hEdma = NULL;
    status = register_tsks(TSK_ctrl_term, &Term_trace, &QUE0, "/UART2", IOM_INPUT|IOM_OUTPUT, &UART_chanParams);
    if(status){
    	//DEBUG
#ifdef _DEBUG
    	LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_ctrl_term);
#endif
    }
    status = register_tsks(TSK_datalog, &sys_trace, &QUE1, NULL, NULL, NULL);	//no extra GIO_Handle required
    if(status){
    	//DEBUG
#ifdef _DEBUG
    	LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_datalog);
#endif
    }
    /* datalog and ctrl_term are sharing the same Uart Tx, since UART PSP driver only allows for 1 channel per instance */
    SCB.TCB[TSK_datalog].outhd = SCB.TCB[TSK_ctrl_term].outhd;

    I2C_chanParams.hEdma = NULL;
    I2C_chanParams.masterOrSlave = I2c_CommMode_MASTER;
    status = register_tsks(TSK_BMA180, &AccSampleTsk, &QUE2, "/I2C0", IOM_INPUT|IOM_OUTPUT,&I2C_chanParams);
    if(status){
    	//DEBUG
#ifdef _DEBUG
    	LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_BMA180);
#endif
    }
    status = register_tsks(TSK_ITG3205, &GyroSampleTsk,&QUE3, "/I2C0", IOM_INPUT|IOM_OUTPUT,&I2C_chanParams);
    if(status){
    	//DEBUG
#ifdef _DEBUG
    	LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_ITG3205);
#endif
    }
    status = register_tsks(TSK_MS5611, &BaroSampleTsk,&QUE4, "/I2C0", IOM_INPUT|IOM_OUTPUT,&I2C_chanParams);
    if(status){
    	//DEBUG
#ifdef _DEBUG
    	LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_MS5611);
#endif
    }
    status = register_tsks(TSK_HMC5883L, &MagSampleTsk,&QUE5,"/I2C0", IOM_INPUT|IOM_OUTPUT,&I2C_chanParams);
    if(status){
    	//DEBUG
#ifdef _DEBUG
    	LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_HMC5883L);
#endif
    }
	status = register_tsks(TSK_GPS, &GpsSampleTsk, &QUE6,"/I2C0", IOM_INPUT|IOM_OUTPUT,&I2C_chanParams);
	if(status){
		//DEBUG
#ifdef _DEBUG
		LOG_printf(SCB.trace, "ERROR: register_tsks ID = %d",TSK_GPS);
#endif
	}
    return;
}

/*
 * Perform essential configuration for task with TSK_ID
 */
Int32 register_tsks(Int32 TSK_ID, LOG_Handle trace, QUE_Handle queue, String dev, Int32 dir, Ptr chanParams){
	Int32 status = 0;
	TSK_Ctrl_b *tsk = NULL;
	/* set up task's TCB - Currently only support static task registration */
	tsk = &(SCB.TCB[TSK_ID]);
	tsk->queue = queue;					/* task message queue IPC */
	tsk->trace = trace;					/* BIOS Log Object associated with task */
	tsk->status = UNINITIALISED;		/* Task self status */
	tsk->len = 0;						/* Actual string length of serial message output buffer */
	if(dev == NULL){
		return 0;				//finish set up here if no dev needed
	}
	//Set up GIO handlers
	if(dir & IOM_INPUT){
		tsk->inhd = GIO_setup(&status, dev, IOM_INPUT, 2, chanParams); /* general input channel */
		if(tsk->inhd == NULL ||SCB.TCB[TSK_ID].inhd == NULL||status){
			LOG_printf(tsk->trace, "ERROR SETTING UP GIO_handler: %s %d", dev, status);
		}
	}
	if(dir & IOM_OUTPUT){
		tsk->outhd = GIO_setup(&status, dev, IOM_OUTPUT, 2, chanParams); /* general output channel */
		if(tsk->outhd == NULL ||SCB.TCB[TSK_ID].outhd == NULL|| status){
#ifdef _DEBUG
			LOG_printf(tsk->trace, "ERROR SETTING UP GIO_handler: %s %d", dev, status);
#endif
		}
	}
	return status;
}


/*--------------central_control_PRD()---------------------------
 * This function is called by PRD module (special SWI), it post
 * the semaphore BMA180_sem to allow the sampling task to runs.
 * NOTE: printf() cannot be used in it.
 *------------------------------------------------------*/
void central_control_PRD(void){

	if(SCB.INITIALISED){

		vUpdateTimeStamp();

		if(TRUE == SCB.TSK_ENA[TSK_BMA180]){
			//allow BMA180_sample_tsk() to run
			SEM_post(&BMA180_sem);
			SCB.TCB[TSK_BMA180].prev_t = SCB.TIME_STAMP;//set time
		}
		if(TRUE == SCB.TSK_ENA[TSK_ITG3205]){
			//allow ITG3200_sample_tsk() to run
			SEM_post(&ITG3200_sem);
			SCB.TCB[TSK_ITG3205].prev_t = SCB.TIME_STAMP;//set time
		}
		if(TRUE == SCB.TSK_ENA[TSK_HMC5883L]){
			//allow HMC5883L_sample_tsk() to run
			SEM_post(&HMC5883L_sem);
			SCB.TCB[TSK_HMC5883L].prev_t = SCB.TIME_STAMP;//set time
		}
		if(TRUE == SCB.TSK_ENA[TSK_MS5611] && tsk_ctrl%3==0){
			//allow MS5611_sample_tsk() to run
			SEM_post(&MS5611_sem);
			SCB.TCB[TSK_MS5611].prev_t = SCB.TIME_STAMP;//set time
		}
		if(TRUE == SCB.TSK_ENA[TSK_datalog] && tsk_ctrl%2==0){
			//allow serial_printf_tsk() to run
			SEM_post(&datalog_sem);
		}
		if(TRUE == SCB.TSK_ENA[TSK_GPS] && tsk_ctrl%20==0){
			//allow serial_printf_tsk() to run
			SEM_post(&GPS_sem);
			SCB.TCB[TSK_GPS].prev_t = SCB.TIME_STAMP;//set time
		}
		if(TRUE == SCB.TSK_ENA[MATRIX_TEST]){
			SEM_post(&matrix_sem);
			//SCB.TCB[MATRIX_TEST].prev_t = SCB.TIME_STAMP;//set time
		}
		tsk_ctrl ++;
	}

}



//return time in ms since start up. Res:1ms
unsigned long vUpdateTimeStamp(void){
	unsigned long Itpcount = 0;
	unsigned long TIME_STAMP;
	Itpcount = (unsigned long)CLK_getltime();
	SCB.TIME_STAMP =  (Itpcount * SCB.CLK_period) / SCB.CLK_pms;
	SCB.TIME_STAMP_H = (short)((SCB.TIME_STAMP >> 16) & 0xFFFF);
	SCB.TIME_STAMP_L = (short)(SCB.TIME_STAMP & 0xFFFF);
	return TIME_STAMP;
}


void vTskTime(TSK_Ctrl_b *tsk, time_op op){

	if(op == RESET_TIME){

		tsk->total_t = 0;
		tsk->count = 0;
		tsk->prev_t = 0;

	}else if(op == DELTA_TIME){

		vUpdateTimeStamp();

		tsk->total_t += (SCB.TIME_STAMP - tsk->prev_t);
		tsk->count ++;
		tsk->prev_t = SCB.TIME_STAMP;

		tsk->avg_t = ((double)tsk->total_t)/((double)tsk->count);

	}else if(op == SET_PREV_TIME){
		vUpdateTimeStamp();
		tsk->prev_t = SCB.TIME_STAMP; //set time
	}
}



/*
 * I2C0 device params. To be filled in user_i2c_init function which
 * is called before driver creation
 */
I2c_Params   i2cParams;
Uart_Params   uartParams;
/*
 * I2C0 init function called when creating the driver.
 */
void user_i2c_init()
{
	//I2c_init initializes the I2C instance handler by doing memset 0
    I2c_init();
    i2cParams = I2c_PARAMS;
    i2cParams.busFreq = 100000;
    i2cParams.hwiNumber = 8;
    /* configure I2c for interrupt mode */
    i2cParams.opMode = I2c_OpMode_INTERRUPT;
}

void user_uart2_init()
{
    Uart_init();
    uartParams = Uart_PARAMS;
    uartParams.hwiNumber = 9;
    uartParams.opMode = Uart_OpMode_INTERRUPT;
    uartParams.rxThreshold = Uart_RxTrigLvl_1;
    uartParams.baudRate = Uart_BaudRate_115_2K;
}
/*
void clk_init(void){
	int timeout = 0; //ms
    int ticks = 0;
    timeout = 100;
    ticks = ms_conv_to_tick(timeout);
    LOG_printf(&trace,"Testing timeout %d ms\n",timeout);
    LOG_printf(&trace,"= %d ticks\n",ticks);

}*/
/*
void standard_time (void){
    unsigned long freq;
    float milliSecsPerIntr, cycles;
    unsigned int timeout_tick = 0;
    unsigned int sleep_time = 10; // ms

	// get frequency of platform in kHz
	freq = GBL_getFrequency();
	/ get cpu cycles per ltime interrupt
	cycles = CLK_cpuCyclesPerLtime();
	// computer milliseconds per ltime interrupt
	milliSecsPerIntr = cycles/(float)freq;
	// compute timeout
	timeout_tick = sleep_time/milliSecsPerIntr;
	printf("Platform freq = %lu\n",freq);
	printf("cpu cycles per ltime = %f\n",cycles);
	printf("milliSecsPerIntr = %f\n",milliSecsPerIntr);
	printf( "timeout_tick = %d\n",timeout_tick);
	vUpdateTimeStamp();
	STS_set(&sts_BMA180,SCB.TIME_STAMP);
	while(1){
		TSK_sleep(timeout_tick);
		STS_delta(&sts_BMA180,SCB.TIME_STAMP);
	}
}*/
